/*
  Record a series of poses, then play them back
  */

#include "ros/ros.h"
#include <irb_msgs/ObjectPose.h>

#include <opencv-2.3.1/opencv2/highgui/highgui.hpp>
#include <opencv-2.3.1/opencv2/imgproc/imgproc.hpp>

#include "rec.h"
#include "guitab.h"

#include <sstream>
#include <vector>
using namespace std;

// The recorder class
RecordController *recorder = NULL;

// GUI STUFF
#define WINDOWNAME "Recorder"

// Gui
Gui *gui = NULL;

// Periodic callback handles time-dependent logic that draws the monitor screen
void periodicCallback(const ros::TimerEvent&)
{
	static ros::Time lasttime = ros::Time::now();
	ros::Time curtime = ros::Time::now();
	ros::Duration deltatime = curtime - lasttime;
	lasttime = curtime;

	// Update time-dependent stuff
	recorder->Update(deltatime);

	// Create image to draw on (y, x, depth)
	cv::Mat image(600, 800, CV_8UC3);
	image = cv::Scalar(255,255,255);

	// Draw GUI
	gui->Draw(image);

	// Draw image to screen
	cv::imshow(WINDOWNAME, image);
	cv::waitKey(1); // Image doesn't show without this line for some reason, some problem with ROS
}

// Mouse callback to capture button clicks and similar
void mouse(int event, int x, int y, int, void* )
{
	cv::Point2d mouse(x,y);
	gui->HandleInput(event, mouse);
}

// Callback when receiving pose from input stream
void poseCallback(const irb_msgs::ObjectPose& msg)
{
	PoseInternal pose(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z,
					  msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w,
					  ros::Time::now());
	recorder->HandlePose(pose, msg.ID);
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "recorder");

	cv::namedWindow(WINDOWNAME);
	cv::setMouseCallback(WINDOWNAME, mouse);

	ros::NodeHandle n;

	Gui::Init();

	// Instansiate the recorder. This will also publish "playback" (private publisher in RecordController)
	recorder = new RecordController(n);
	gui = new Gui(recorder);

	// Subscribe to input stream
	ros::Subscriber sub = n.subscribe("pose", 1000, poseCallback);
	// Start periodic drawing
	ros::Timer timer = n.createTimer(ros::Duration(0.1), periodicCallback);

	ros::spin();

	return 0;
}
